x_f = 20.0;
y_f = 20.0;
yaw_f = pi/3;
ga_angle = 0;
mode = 1;

SANY_ARR = ARR_Kinematic_Model(x_f,y_f,yaw_f,ga_angle,mode);
% X_F = 1:1:100;
% Y_F = 1:1:100;
% scatter(SANY_ARR.P_f.x,SANY_ARR.P_f.y,'red');
% hold on;
% scatter(SANY_ARR.P_m.x,SANY_ARR.P_m.y,'blue');
% hold on;
% scatter(SANY_ARR.P_r.x,SANY_ARR.P_r.y,'green');
% hold on;
% line([SANY_ARR.P_f.x,SANY_ARR.P_m.x],[SANY_ARR.P_f.y,SANY_ARR.P_m.y],'Color','black');
% hold on;
% line([SANY_ARR.P_m.x,SANY_ARR.P_r.x],[SANY_ARR.P_m.y,SANY_ARR.P_r.y],'Color','black');
% hold on;

sc_f = scatter(SANY_ARR.P_f.x,SANY_ARR.P_f.y,'red');
hold on;
sc_m = scatter(SANY_ARR.P_m.x,SANY_ARR.P_m.y,'blue','.');
hold on;
sc_r = scatter(SANY_ARR.P_r.x,SANY_ARR.P_r.y,'green');
hold on;
l_f = line([SANY_ARR.P_f.x,SANY_ARR.P_m.x],[SANY_ARR.P_f.y,SANY_ARR.P_m.y],'Color','black');
hold on;
l_r = line([SANY_ARR.P_m.x,SANY_ARR.P_r.x],[SANY_ARR.P_m.y,SANY_ARR.P_r.y],'Color','black');
hold on;
% sc_f1 = scatter(SANY_ARR.P_f_c1.x,SANY_ARR.P_f_c1.y,'black');
% hold on;
% sc_f2 = scatter(SANY_ARR.P_f_c2.x,SANY_ARR.P_f_c2.y,'black');
% hold on;

l_f1 = line([SANY_ARR.P_f_c2.x,SANY_ARR.P_f_c1.x],[SANY_ARR.P_f_c2.y,SANY_ARR.P_f_c1.y],'Color','black');
hold on;
l_f2 = line([SANY_ARR.P_f_c2.x,SANY_ARR.P_f_c3.x],[SANY_ARR.P_f_c2.y,SANY_ARR.P_f_c3.y],'Color','black');
hold on;
l_f3 = line([SANY_ARR.P_f_c3.x,SANY_ARR.P_f_c4.x],[SANY_ARR.P_f_c3.y,SANY_ARR.P_f_c4.y],'Color','black');
hold on;
l_f4 = line([SANY_ARR.P_f_c4.x,SANY_ARR.P_f_c1.x],[SANY_ARR.P_f_c4.y,SANY_ARR.P_f_c1.y],'Color','black');
hold on;

l_r1 = line([SANY_ARR.P_r_c2.x,SANY_ARR.P_r_c1.x],[SANY_ARR.P_r_c2.y,SANY_ARR.P_r_c1.y],'Color','black');
hold on;
l_r2 = line([SANY_ARR.P_r_c2.x,SANY_ARR.P_r_c3.x],[SANY_ARR.P_r_c2.y,SANY_ARR.P_r_c3.y],'Color','black');
hold on;
l_r3 = line([SANY_ARR.P_r_c3.x,SANY_ARR.P_r_c4.x],[SANY_ARR.P_r_c3.y,SANY_ARR.P_r_c4.y],'Color','black');
hold on;
l_r4 = line([SANY_ARR.P_r_c4.x,SANY_ARR.P_r_c1.x],[SANY_ARR.P_r_c4.y,SANY_ARR.P_r_c1.y],'Color','black');
hold on;

axis([0 400 0 400]);
hold on;
drawnow;
% X_F(1)=SANY_ARR.P_f.x;
% Y_F(1)=SANY_ARR.P_f.y;
sw_enable = 1;
for i = 2 : 800
    ga_angle_rate = 0;
%     if i > 300
%         ga_angle_rate = -pi/36;
%     else
%         ga_angle_rate = pi/6;
%     end
    
    if mod(i,50) == 0
        sw_enable = -1*sw_enable;
    end
    fprintf('i %d sw_enable %d \n',i,sw_enable);
    if sw_enable == 1
        ga_angle_rate = -pi/6;
    else
        ga_angle_rate = pi/6;
    end
    
        fprintf("angle rate is %f \n",ga_angle_rate);
    SANY_ARR.State_Update(-5, 0.1, ga_angle_rate);
    set( sc_f, 'XData', SANY_ARR.P_f.x, 'YData', SANY_ARR.P_f.y );
    set( sc_m, 'XData', SANY_ARR.P_m.x, 'YData', SANY_ARR.P_m.y );
    set( sc_r, 'XData', SANY_ARR.P_r.x, 'YData', SANY_ARR.P_r.y );
    set( l_f, 'XData', [SANY_ARR.P_f.x,SANY_ARR.P_m.x], 'YData', [SANY_ARR.P_f.y,SANY_ARR.P_m.y] );
    set( l_r, 'XData', [SANY_ARR.P_m.x,SANY_ARR.P_r.x], 'YData', [SANY_ARR.P_m.y,SANY_ARR.P_r.y] );
    
    set( l_f1, 'XData', [SANY_ARR.P_f_c2.x,SANY_ARR.P_f_c1.x], 'YData', [SANY_ARR.P_f_c2.y,SANY_ARR.P_f_c1.y] );
    set( l_f2, 'XData', [SANY_ARR.P_f_c2.x,SANY_ARR.P_f_c3.x], 'YData', [SANY_ARR.P_f_c2.y,SANY_ARR.P_f_c3.y] );
    set( l_f3, 'XData', [SANY_ARR.P_f_c3.x,SANY_ARR.P_f_c4.x], 'YData', [SANY_ARR.P_f_c3.y,SANY_ARR.P_f_c4.y] );
    set( l_f4, 'XData', [SANY_ARR.P_f_c4.x,SANY_ARR.P_f_c1.x], 'YData', [SANY_ARR.P_f_c4.y,SANY_ARR.P_f_c1.y] );
    
    set( l_r1, 'XData', [SANY_ARR.P_r_c2.x,SANY_ARR.P_r_c1.x], 'YData', [SANY_ARR.P_r_c2.y,SANY_ARR.P_r_c1.y] );
    set( l_r2, 'XData', [SANY_ARR.P_r_c2.x,SANY_ARR.P_r_c3.x], 'YData', [SANY_ARR.P_r_c2.y,SANY_ARR.P_r_c3.y] );
    set( l_r3, 'XData', [SANY_ARR.P_r_c3.x,SANY_ARR.P_r_c4.x], 'YData', [SANY_ARR.P_r_c3.y,SANY_ARR.P_r_c4.y] );
    set( l_r4, 'XData', [SANY_ARR.P_r_c4.x,SANY_ARR.P_r_c1.x], 'YData', [SANY_ARR.P_r_c4.y,SANY_ARR.P_r_c1.y] );
% set( sc_f1, 'XData', SANY_ARR.P_f_c1.x, 'YData', SANY_ARR.P_f_c1.y );
% set( sc_f2, 'XData', SANY_ARR.P_f_c2.x, 'YData', SANY_ARR.P_f_c2.y );
    drawnow;
    scatter(SANY_ARR.P_f.x,SANY_ARR.P_f.y,'red','.');
    hold on;
    scatter(SANY_ARR.P_m.x,SANY_ARR.P_m.y,'blue','.');
    hold on;
    scatter(SANY_ARR.P_r.x,SANY_ARR.P_r.y,'green','.');
    hold on;
%     line([SANY_ARR.P_f.x,SANY_ARR.P_m.x],[SANY_ARR.P_f.y,SANY_ARR.P_m.y],'Color','black');
%     hold on;
%     line([SANY_ARR.P_m.x,SANY_ARR.P_r.x],[SANY_ARR.P_m.y,SANY_ARR.P_r.y],'Color','black');
%     hold on;
%     X_F(i)=SANY_ARR.P_f.x;
%     Y_F(i)=SANY_ARR.P_f.y;
    pause(0.001);
end